import rclpy
from rclpy.node import Node
import rclpy.time
from tf2_ros import TransformListener,Buffer
from geometry_msgs.msg import TransformStamped
from tf_transformations import euler_from_quaternion # 4元素函数转欧拉角
import math

class TfBrodercaster(Node):
    def __init__(self):
        super().__init__('tf_listener')
        self.buffer_ = Buffer()
        self.listener_= TransformListener(self.buffer_,self)
        self.timer_ = self.create_timer(1.0,self.get_transform)

    def get_transform(self):
        try:
            result:TransformStamped
            result = self.buffer_.lookup_transform('base_link','bottle_link',
                                                   rclpy.time.Time(seconds=0),# 表示查询最新
                                                   rclpy.time.Duration(seconds=1.0) # 一分钟超时
                                                   )
            transform = result.transform
            self.get_logger().info(f'平移{transform.translation}')
            self.get_logger().info(f'旋转{transform.rotation}')
            rotate_enler = euler_from_quaternion([
                transform.rotation.x,
                transform.rotation.y,
                transform.rotation.z,
                transform.rotation.w,
            ]
            )
            self.get_logger().info(f'旋转rpy{rotate_enler}')

        except Exception as e: 
            self.get_logger().info(f'获取坐标失败，原因{str(e)}')
            pass


def main():
    rclpy.init()
    node = TfBrodercaster()
    rclpy.spin(node)
    rclpy.shutdown()